import numpy as np
import math

def mat_update(prev_mat, mat):
    if np.linalg.det(mat) == 0:
        return prev_mat, False # Return previous matrix and False flag if the new matrix is non-singular (determinant ≠ 0).
    else:
        # 将VR的坐标系(xyz:右上后)
        return mat, True


def fast_mat_inv(mat):
    ret = np.eye(4)
    ret[:3, :3] = mat[:3, :3].T
    ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3]
    return ret


def rotation_matrix_to_euler_angles(R):
    """
    将旋转矩阵转换为欧拉角（roll, pitch, yaw）
    """
    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6

    if not singular:
        roll = math.atan2(R[2, 1], R[2, 2])
        pitch = math.atan2(-R[2, 0], sy)
        yaw = math.atan2(R[1, 0], R[0, 0])
    else:
        roll = math.atan2(-R[1, 2], R[1, 1])
        pitch = math.atan2(-R[2, 0], sy)
        yaw = 0

    # 转换为角度
    return np.array([roll, pitch, yaw]) * 180.0 / np.pi


def print_pose_info(pose_matrix):
    """
    打印位姿矩阵的详细信息，包括位置和姿态
    """
    # 提取位置信息（前三行第四列）
    position = pose_matrix[:3, 3]
    
    # 提取旋转矩阵（前三行前三列）
    rotation = pose_matrix[:3, :3]
    
    # 将旋转矩阵转换为欧拉角
    euler_angles = rotation_matrix_to_euler_angles(rotation)
    
    return {
        "position": {
            "x": round(position[0], 3),
            "y": round(position[1], 3),
            "z": round(position[2], 3)
        },
        "orientation": {
            "roll": round(euler_angles[0], 3),
            "pitch": round(euler_angles[1], 3),
            "yaw": round(euler_angles[2], 3)
        }
    }


def test_mat_update():
    """测试mat_update函数的坐标系转换"""
    # 测试1: 测试位置转换
    # VR坐标系中的位置 (x=1, y=2, z=3)
    # 应该转换为机器人坐标系的 (y=2, z=3, x=-1)
    test_mat = np.eye(4)
    test_mat[:3, 3] = [1, 2, 3]  # 设置平移部分
    
    # 使用单位矩阵作为prev_mat，它应该只在mat为奇异矩阵时被返回
    prev_mat = np.eye(4)
    
    # 测试非奇异矩阵
    result_mat, success = mat_update(prev_mat, test_mat)
    assert success, "转换应该成功"
    
    # 验证位置转换
    expected_position = [-2, 3, -1]  # y, z, -x
    np.testing.assert_almost_equal(
        result_mat[:3, 3],
        expected_position,
        decimal=6,
        err_msg=f"位置转换错误: 期望 {expected_position}, 得到 {result_mat[:3, 3]}"
    )
    
    # 测试2: 测试旋转矩阵转换
    # 创建一个绕x轴旋转45度的旋转矩阵
    theta = np.radians(45)
    rot_x = np.array([
        [1, 0, 0, 0],
        [0, np.cos(theta), -np.sin(theta), 0],
        [0, np.sin(theta), np.cos(theta), 0],
        [0, 0, 0, 1]
    ])
    
    # 应用转换
    transformed_rot, _ = mat_update(prev_mat, rot_x)
    
    # 验证旋转矩阵的性质（正交矩阵，行列式为1）
    assert np.allclose(
        transformed_rot[:3, :3] @ transformed_rot[:3, :3].T,
        np.eye(3),
        atol=1e-6
    ), "转换后的旋转矩阵不是正交矩阵"
    
    assert np.isclose(
        np.linalg.det(transformed_rot[:3, :3]),
        1.0,
        atol=1e-6
    ), "转换后的旋转矩阵行列式不为1"
    
    # 测试3: 测试奇异矩阵情况
    singular_mat = np.zeros((4, 4))
    result, success = mat_update(prev_mat, singular_mat)
    assert not success, "奇异矩阵应该返回False"
    assert np.array_equal(result, prev_mat), "奇异矩阵应该返回prev_mat"
    
    print("所有测试通过！")


if __name__ == "__main__":
    test_mat_update()
